#ifndef COMPENSATOR_H
#define COMPENSATOR_H

#include "hnurm_armor/DataType.hpp"

#include <hnurm_interfaces/msg/vision_recv_data.hpp>
#include <hnurm_interfaces/msg/vision_send_data.hpp>

#include <opencv2/core.hpp>
#include <opencv2/core/types.hpp>
#include <opencv2/opencv.hpp>
#include <rclcpp/rclcpp.hpp>

namespace hnurm
{
    class Compensator
    {
    private:
        // 默认参数
        float gravity_ = 9.8;
        float friction_coeff_ = 0.02;
        float bullet_speed_bias_ = -0.5;
        float imutofrt_y = 0.0;
        float imutofrt_z = 0.0;
        TargetInfo t_info;
        float fx, fy, fz, fx_center, fy_center, fv, yaw_difference;
        float pout, yout;
        float tmp_fly_time;
        float bullet_speed_;
        float channel_delay_;
        std::vector<float> px;
        std::vector<float> py;
        std::vector<float> pz;
        std::vector<float> pv;

        void PredictPose(ArmorsNum &armors_num);

        void PredictPosition();

        void CompensatePitch();

        float calculateAverage(const std::vector<float> &vec);

        void Averagedistance(std::vector<float> &p, float value);

        [[nodiscard]] double diff_eq(double v) const;

        double integrate_euler(double v0, double t);

    public:
        explicit Compensator(rclcpp::Node::SharedPtr node);

        float CalcFinalAngle(
            TargetInfo &target_msg,
            hnurm_interfaces::msg::VisionRecvData &recv_data,
            hnurm_interfaces::msg::VisionSendData &send_data,
            ArmorsNum &num
        );

        struct predict_armor
        {
            cv::Point3f position;
            float yaw;
        };

        static bool cmp(const predict_armor &a, const predict_armor &b);

        float yaw_bias_ = 0.0;
        float pitch_bias_ = 0.0;
    private:
        rclcpp::Node::SharedPtr node_;
    };
}  // namespace hnurm

#endif  // COMPENSATOR_H